#define DEBUG_PROJ 0
#define ENABLE    1 
#define COUNTER_ENABLE 1

#include <ctl_api.h>
#include <stdio.h>
//#include <stdint.h>
#include "freeRTOS.h"
#include "task.h"
#include "Vector.h"
#include "pwm.h"
#include "pushBrake.h"
#include "watchdog.h"

extern "C"
{
  #include <string.h>
  #include "main.h"
  #include "Logomatic_V2.h"
  #include "ADC.h"
  //#include "i2c.h"
}

//#define ACCEL_THRESHOLD     50.0f
//#define MIN_THRESHOLD       10.0f
//#define ERROR_THRESHOLD     5.0f
//#define MESG_LENGTH         6

#define TASK_STACK_SIZE             300
#define PID_TASK_PRIORITY           4
#define Hall_TASK_PRIORITY          6
#define Timer_1ms_TASK_PRIORITY     5
#define LOGGING_TASK_PRIORITY       3
#define COUNTING_TASK_PRIORITY      2

//const unsigned long crp __attribute__ ((at(0x2FC))) = 0x87654321;

#define UART_PINSEL0_TXD0 0b01
#define UART_PINSEL0_RXD0 (0b01<<2)
#define UART_PINSEL0_TXD1 (0b01<<16)
#define UART_PINSEL0_RXD1 (0b01<<18)
static void uart1ISR (void) __attribute__ ((naked));
void uart1ISR_handler (void);

//#define PINSEL0_SCL0 (0B01<<4)
//#define PINSEL0_SDA0 (0b01<<6)
//#define PINSEL0_SCL1 (0B11<<22)
//#define PINSEL0_SDA1 (0b11<<28)



static void hallISR (void) __attribute__ ((naked));
void hallISR_handler (void);
void hallISR_init(void);
#define HALL_A_BIT (1<<24)
#define HALL_B_BIT (1<<25)
#define HALL_C_BIT (1<<23)   //board 2.10 use p1.23, 2.09 use p1.26
//#define HALL_C_BIT (1<<26)
#define PH_A_HIGH_BIT (1<<16)
#define PH_A_LOW_BIT (1<<17)
#define PH_B_HIGH_BIT (1<<18)
#define PH_B_LOW_BIT (1<<19)
#define PH_C_HIGH_BIT (1<<20)
#define PH_C_LOW_BIT (1<<21)

bool motorShutDownRecovery = false;
void set_motorState(bool _state);
bool get_motorState(void);
void motorHallSensConfig(void);
void motorPhasesInit(void);
void motorPhasesConfig(void);
void motorPhasesClearAll(void);

#define POWER_ON_BIT (1<<10)
#define POWER_LED_BIT (1<<26)
#define L_POWER_BIT (1<<5)
#define FAN_BIT    (1<<20)

//void WDT_init(void);
//void WDT_service(void);

#define BUZZER_ON  1 
#define BUZZER_OFF 0 
void buzzer (char buzz);
void buzzer_logic(void);
void buzzer_loadSingleBeep(void);
void buzzer_loadDoubleBeep(void);
void buzzer_loadNoBeep(void);
char Buzzer_delay2=0;
char Buzzer_delay1=0;
//void set_pwm(unsigned pwm_duty);
//void pwm_init(void);


//float get_acc_z(void);

////i2c
#define ACCEL_ADDR 0b00111000
#define GYRO_ADDR 0b11010000

#define OUT_X_LSB 0x05
#define OUT_X_MSB 0x06
#define OUT_Y_LSB 0x07 
#define OUT_Y_MSB 0x08
#define OUT_Z_LSB 0x09 
#define OUT_Z_MSB 0x0A
#define OUT_X_GYRO 0xA8
#define READ_GYRO 1
#define READ_ACCEL 0
#define DATA_LENGTH 6

#define CTRL_REG1 0x38
#define ACC_XYZ_DATA_CFG 0x16

#define ACCEL_INIT_MASK 0b00001101  //50hz +-2g
int tilt_x=1024;
int tilt_y=1024;

void I2CISR_handler(void);

float acc_cal_x(void);
float acc_cal_z(void);
float acc_cal_y(void);
char acc_test(void);

void i2c_init(void);
void i2c_acc_init(void);
void i2c_gyro_init(void);
void get_acc_data(void);
void get_gyro_data(void);

unsigned I2CAddress;
unsigned I2C_reg_Address;
unsigned I2CData;

char i2cact=0;
char write_count=0;
char read_count=0;
char multi_read=0;
char accel_data[DATA_LENGTH];
char gyro_data[DATA_LENGTH];
float acc_x=0;
float acc_y=0;
float acc_z=0;
char i2c_bus_busy=0;


//int precharge_delay=200;
//unsigned int pwm_duty=2;

bool PID_lowerTransientEvent = false;
bool PID_lowerTransientEventLockout = false;
bool PID_lowerTransientEventSendLock = false;
unsigned int PID_lowerTransientEventLockoutTimer = 0;
bool PID_lowerTransientWheelieEvent = false;
bool PID_lowerTransientWheelieEventLockout = false;
bool PID_lowerTransientWheelieEventSendLock = false;
unsigned int PID_lowerTransientWheelieEventLockoutTimer = 0;

#define PID_RATIO_CONSTANT 19.0f
#define PID_CONSTANT        40.0f

//int gyro_buff_size=10;
#define GYRO_BUFF_SIZE 6
int gyro_buff_index=0;
int16_t gyro_buff[10];
int32_t gyro_buffSum=0;
int32_t gyro_posSum = 0;
int32_t gyro_negSum = 0;
int32_t gyro_absSum = 0;
int32_t gyro_avg=0;

#define GYRO_MAX_THRESH_1 2500
#define GYRO_MIN_THRESH_1 -2500
#define GYRO_MAX_THRESH_2 4000//2500
#define GYRO_MIN_THRESH_2 -4000//-2500
#define GYRO_DIFF_THRESH 5000//4000
#define GYRO_NEG_SUM_THRESH -4000
#define GYRO_POS_SUM_THRESH -4000
  
  int32_t gyro_buffMin = 0;
  int32_t gyro_buffMax = 0;
  int32_t gyro_buffDiff = 0;

//i2c end


// uartISR helper functions
inline char UA_decodeNewDriveMode(void);
inline char UA_decodeButtonState(void);
int UA_getTargetButtonState(void);
void UA_checkNewDriveMode(void);
void UA_checkForUpperTransient(void);
void UA_button_stateMachine(void);
void UA_drive_stateMachine(void);
void UA_readSwitches(void);
void UA_readSwitchesOld(void);
void UA_updateButtonPressedCounter(void);;
inline int UA_getButtonPressedCounter(void);
void UA_transPressedToReleased(void);

// PIDTask helper functions
void PID_pushMode_turnMotorOffReset(void);
void PID_pushMode_updatePreviousSpeedSetpoint(void);
void PID_pushMode_unstableSpeedControl(void * pidParameters);
void PID_pushMode_stableSpeedControl(void * pidParameters);
void PID_pushMode_speedControlSM(void * pidParameters);
void PID_pushMode_enterButtonCruise(void);
void PID_pushMode_motorRunning(void * pidParameters);
//void CT_pushMode_disable(void);

inline bool PID_pushMode_detectBrakeAfterPush(void);
//inline bool PID_pushMode_isMaxAccelUnderThresh(void);
inline float PID_pushMode_getBrakeRatio(void);
inline bool PID_pushMode_detectBrakeWhenStable(void);
inline bool PID_pushMode_detectPushInPush(void);
inline bool PID_pushMode_detectValidPushInPush(void);
void PID_pushMode_respondToBrake(void);

void PID_manualMode_runMotorMaxSpeed(float temp_spd);
void PID_manualMode_runMotorButtonOn(void);
void PID_manualMode_stateMachine(void);
void PID_manualMode_motorRunning(void * pidParameters);

void PID_xMode_turnMotorOff(void);
void PID_timeOut_logic(void * pidParameters);

void debugDataStream_init(void);
void debugDataStream_transmit(void * pvParameters);
void PID_battCurrentFilter(void);


void PID_checkForLowerTransient(void);
void PID_setGyroTransient(void);
void PID_setGyroWheelieTransient(void);
void PID_lowerTransient(void);



/***************************/   
/* PID Control Variables   */
/***************************/
//  float _percent_error = 0.0f;
//  float error_percent[4];

unsigned int pid_delay=0;
  
//  float previous_error = 0.0f;
//  float integral=0,derivative=0,error=0;
float PID_output = 0.0f;
unsigned int PID_duty_unstable = 0;
unsigned int PID_duty_stable = 0;
bool PID_stable=false;
float PID_prevSpeedSetpoint = 1.0f;

const float PID_BRAKE_PROP  = 1.10f;
const float PID_PUSH_PROP   = 0.9f;
float PID_current_brake_ratio =0.0f;
float PID_speed_ratio = 2.0f;
float PID_current_ratio = 0.0f;
float PID_previous_ratio = 0.0f;
unsigned int PID_manual_output=0;
int UA_pressed_delay=0;
int new_press=0;
int UA_cruse=0;
float UA_cruse_speed=0;
int PID_timeout_count=0;
static unsigned int PID_temperature = 400;
static unsigned int PID_battCurrent = 0;
static unsigned int PID_battVoltage = 0;
static unsigned int PID_battChallenger = 0;
static unsigned int PID_temperature_new, PID_battCurrent_new;

//int dac_output=0;
//float curr_prop_sw = 0.0f;
//float speed_prop_sw = 0.0f;
//int manual_step=40;
//float Prev_Integral = 0.0f;
//float stable_SetPoint = 1.0f;
//float stable_Integral = 2.3f;
//float pre_speed;
//char press_speed = 0;    //when press button with speed >1mph, use high acc
// button pressed coasting time ( not used)
//#define HIGH_COAST_TIME   90
//#define NORMAL_COAST_TIME 60
//#define LOW_COAST_TIME    30
//static int COAST_TIME = NORMAL_COAST_TIME;        // defined in 10ths of a second (50=5seconds)
//static int coasting_time = 0;



/***************************/   
/* UART Variables   */
/***************************/
#define UPPER_DATA_LENGTH 50
char upper_data_stream[UPPER_DATA_LENGTH];
char upper_data[13];
//char data_read_index=0;
char data_write_index=0;

/***************************/   
/* Motor Variables   */
/***************************/
bool motor_run=false;


/***************************/   
/* buttonState and DriveMode Variables   */
/***************************/
#define MANUAL_MODE  1
#define AUTO_MODE    0
#define OFF_MODE     2
static int UA_drive_mode = MANUAL_MODE;
bool UA_upperTransientEvent = false;
bool UA_upperTransientEventLockout = false;
unsigned int UA_upperTransientEventLockoutTimer = 0;


#define SWITCH_PRESS   1
#define SWITCH_RELEASE 0
#define SWITCH_COAST   2
#define SWITCH_STOP    3

int button_targetState = SWITCH_RELEASE;


//int _adcval;
//int pre_adcval;
//int pressed=0;

typedef struct
{
   uint32_t frequency;            // Blinking frequency
} sTaskParams_t, *spTaskParams_t;

typedef struct
{
   float Kp,          // proportional constant
         Ki,          // integral constant
         Kd;          // derivative constant
   float CT_speedSetpoint;    // CT_speedSetpoint for PID
   uint32_t frequency;// frequency for task
} sPIDParams_t, *spPIDParams_t;

sTaskParams_t CountingArgs = {
                            10    // Task Execution Frequency
                            };

sTaskParams_t HallArgs =    {
                            2000    // Task Execution Frequency
                            };
sTaskParams_t Timer1msArgs =    {
                            1000    // Task Execution Frequency
                            };
sPIDParams_t PIDArgs =      {
                            10,   // Kp
                            6,    // Ki
                            2,    // Kd
                            0,    // initial speed CT_speedSetpoint
                            10    // Task Execution Frequency
                            };

extern "C"
{
  int           main              ( void );
  void          bootup            ( void );
  void          PllFeed           ( void );
  static void   initTimer1        ( void );
  static int    CRC               (char* msg,int len);
  
  void          uart0_tx          (char data);
  void          uart1_tx          (char data);
  void          hall_eintc_h           (void);
  void          hall_eintc_l           (void);
  void          hall_eintb_h           (void);
  void          hall_eintb_l           (void);
  void          hall_einta_h           (void);
  void          hall_einta_l           (void);
}

xTaskHandle xCountingTaskHandle,
            xPIDTaskHandle,
            xTimer_1msHandle;
static void   CountingTask        ( void *pvParameters );
static void   PIDTask             ( void *pvParameters );
static void   Timer_1msTask       ( void *pvParameters );

void          vTimer1ISRWrapper   ( void ) __attribute__ ( ( naked ) );
void          vTimer1Handler      ( void ) __attribute__ ( ( noinline ) );
void          UNDEF_Routine       ( void ) __attribute__ ( ( interrupt ( "UNDEF" ) ) );

extern "C" void abort( void )
{
  while (1 == 1)
  {}
} // abort

extern "C" void bootup( void )
{
////void PID_timeOut_logic(void * pidParameters);
////inline bool PID_pushMode_detectBrakeAfterPush(void);
//
////  PID_data pidStruct = {0, 0.0f, 0, 0, false, false, 1.0f, 0.0f, 2.0f, 0.0f, 0.0f, 0, 0, 0}; 
// //         PID_manualMode_runMotorButtonOn, CT_butterworth_filter_update, PID_timeOut_logic, PID_pushMode_detectBrakeAfterPush};
//  CountingStruct_t  CTstruct;
//  PID_t pidStruct;
//  PID_interface pidIntf = {pidStruct, (int*)&CTstruct, pid_init, PID_manualMode_runMotorButtonOn, CT_butterworth_filter_update, PID_timeOut_logic, PID_pushMode_detectBrakeAfterPush};
//  pidStruct = pidIntf.init();
//  //pidIntf.manualMode_runMotorButtonOn();
//  //pidIntf.butterworth_filter_update(0);
//  PWM_t pwmA = pwm_Create(10); // = {0, 0}
//  
//  // PWM_p pwmB;
//  // pwm_init_p(pwmB);
//
//// // pwmA.duty_target = 1;
//    
//  //  pidStruct.manualMode_runMotorButtonOn();
//  //  pidStruct.butterworth_filter_update(0);
//  pidIntf.timeOut_logic(&PIDArgs);
//  pidIntf.pushMode_detectBrakeAfterPush();
////    void (*timeOut_logic)(void*);
////    bool (*pushMode_detectBrakeAfterPush)(void);
  portENTER_CRITICAL();
  {
    SCS=3;      //set GPIO

    // Setting Multiplier and Divider values
    PLLCFG=0x25; // M = 6
    PllFeed();
    
    // Enabling the PLL */
    PLLCON=0x1;
    PllFeed();
    
#define PLOCK 0x400
    // Wait for the PLL to lock to set frequency
    while(!(PLLSTAT & PLOCK)) ;
    
    // Connect the PLL as the clock source
    PLLCON=0x3;
    PllFeed();
    
    // Enabling MAM and setting number of clocks used for Flash memory fetch (4 cclks in this case)
    //MAMTIM=0x3; //VCOM?
    MAMCR=0x2;
    MAMTIM=0x4; //Original
    
    // Setting peripheral Clock (pclk) to System Clock (cclk)   72Mhz
    VPBDIV=0x1;

//    P0DIR |= (1<<STAT1)|(1<<STAT0);
    
//    PINSEL1 |= (2<<18);  // AOUT is on P0.25 (logomatic P4)
//    SCS= 0b11;
    #define TDO 1<<27
    #define TDI 1<<28
    #define TCK 1<<29
    #define TMS 1<<30
    //PINSEL1 = 0;
 //   P1DIR &= ~(3);
 //   PINSEL2 &= ~((1<<2)|(1<<3));
//    P1DIR |= ((TDO)|(TDI)|(TCK)|(TMS));
//    P1SET |= TDI | TDO | TCK | TMS;
    
    PINSEL0 = 0;
    P0DIR &= ~(3);
    P0DIR |= ((1<<12)|(1<<30));
  }
  portEXIT_CRITICAL();
}

extern "C" void PllFeed( void )
{
    PLLFEED=0xAA;
    PLLFEED=0x55;
}

static int CRC(char* msg,int len)
{
  int crc = 65535;
  int i,j;
  for (i=0;i<len;i++)
  {
    crc ^= msg[i];
    for (j=0;j<8;j++)
    { 
      if ( !(crc & 0x01) )
      {
        crc = crc>>1;
        crc &= 0x7FFF;
      }
      else 
      {
        crc = crc>>1;
        crc &= 0x7FFF;
        crc ^= 0xA001;
      }
    }
  }
  return crc;
}

void uart0ISR (void)
{
  char data;
  switch(U0IIR& 0x0f)
  {
    case 0b0100:
    {
      data=U0RBR&0xFF;
    }
    break;

    default:
    break;
  }
}

void uart1ISR_handler (void)
{
  char data;
  char msg[15];
  int crc;
//  static int turn = 0;

//  P0SET = 1<<30;
//   P1CLR |= TCK;//TCK | TMS; //TDI | TDO | TCK | TMS;        
  switch(U1IIR& 0x0f)
  {
    case 0b0100:
    {
      data=U1RBR&0xFF;
  
      if(data_write_index==50)
      data_write_index=0;
      upper_data_stream[data_write_index]=data;
  
      if(upper_data_stream[data_write_index]==0)
      {
        
        if((upper_data_stream[((data_write_index+50-1)%50)]==0xff)&&
        (upper_data_stream[((data_write_index+50-17)%50)]==0x00)&&
        (upper_data_stream[((data_write_index+50-18)%50)]==0xff))
          {
            for(int i=0;i<15;i++)
            {
              msg[i]=upper_data_stream[((data_write_index+50-16+i)%50)];
            }
            crc=CRC(msg,13);
//            if (turn == 0)
//              {
//                P1CLR |= TDI;//TCK | TMS;//TDI | TDO | TCK | TMS;
//                turn = 1;
//                }
//              else 
//              {
//                P1SET |= TDI;//TCK | TMS;//TDI | TDO | TCK | TMS;
//                turn = 0;
//              }
            if (crc==((msg[13]<<8)|msg[14]))
            {
               for(int i=0;i<13;i++)
              {
                upper_data[i]=msg[i];
              } 
//              UA_readSwitchesOld();
              
//              buzzer(1);
              UA_readSwitches();
//              buzzer(0);
              
              
            }
          }
      }
      data_write_index++;
    }
    break;

    default:
    break;
  }
//  P1SET |= TCK;//TCK | TMS;//TDI | TDO | TCK | TMS;
  VICVectAddr=0x00;
  
}

static void uart1ISR (void)
{
  portSAVE_CONTEXT ();
  uart1ISR_handler();
  portRESTORE_CONTEXT();
}

void hall_eintc_h (void)
{
EXTPOLAR |= HALL_C_INT_LEVEL_HIGH_MASK;     //EINT3 HIGH LEVEL TRIGGE
}
void hall_eintc_l (void)
{
EXTPOLAR &= ~HALL_C_INT_LEVEL_HIGH_MASK;   //ENIT3 LOW LEVEL TRIGGER
}
void          hall_eintb_h           (void)
{
EXTPOLAR |= HALL_B_INT_LEVEL_HIGH_MASK;     //EINT0 HIGH LEVEL TRIGGER
}
void          hall_eintb_l           (void)
{
EXTPOLAR &= ~HALL_B_INT_LEVEL_HIGH_MASK;   //ENIT0 LOW LEVEL TRIGGER
}
void          hall_einta_h           (void)
{
EXTPOLAR |= HALL_A_INT_LEVEL_HIGH_MASK;     //EINT0 HIGH LEVEL TRIGGER
}
void          hall_einta_l           (void)
{
EXTPOLAR &= ~HALL_A_INT_LEVEL_HIGH_MASK;   //ENIT2 LOW LEVEL TRIGGER
}

void hallISR_init(void)
{
  HALL_A_INT_PINSEL  |= (HALL_A_INT_SEL_MASK);  //EINT2 for Phase A
  HALL_B_INT_PINSEL  &= ~(0B11<<0);
  HALL_B_INT_PINSEL  |= (HALL_B_INT_SEL_MASK); //EINT0 for Phase B
  HALL_C_INT_PINSEL  &= ~(0B11<<28);
  HALL_C_INT_PINSEL  |= (HALL_C_INT_SEL_MASK);  //EINT3 for Phase C
  EXTMODE  |= HALL_A_INT_EDGE_SENSITIVE | HALL_B_INT_EDGE_SENSITIVE | HALL_C_INT_EDGE_SENSITIVE;// EDGE SENSITIVE FOR EINT0, EINT2, EINT3
  EXTPOLAR |= HALL_A_INT_LEVEL_HIGH_MASK | HALL_B_INT_LEVEL_HIGH_MASK | HALL_C_INT_LEVEL_HIGH_MASK;  // HIGH LEVEL TRIGGER FOR FIRST INTERRUPT
  
  VICIntSelect = 0x00;  
  HALL_A_VICVectCnt = 0x20 | HALL_A_VIC_CH;    //EINT2
  HALL_B_VICVectCnt = 0x20 | HALL_B_VIC_CH;    //EINT0
  HALL_C_VICVectCnt = 0x20 | HALL_C_VIC_CH;    //EINT3
  HALL_A_VICVectAddr = (long)hallISR;
  HALL_B_VICVectAddr = (long)hallISR;
  HALL_C_VICVectAddr = (long)hallISR;
  VICIntEnable |= ((1<<HALL_A_VIC_CH)|(1<<HALL_B_VIC_CH)|(1<<HALL_C_VIC_CH));   // ISR ENABLE
}

static void hallISR (void)
{
  portSAVE_CONTEXT ();
  hallISR_handler();
  portRESTORE_CONTEXT();
}
  
void hallISR_handler (void)
{
  static int hall_val=0;
  static int hall_val_last = 0;
  int pinval = P1PIN;
  hall_val = ( (pinval&HALL_A_BIT) | (pinval&HALL_B_BIT) | ((pinval&HALL_C_BIT)<<3) )>>24 ;// read hall a b c
  //hall_val = ( (pinval&HALL_A_BIT) | (pinval&HALL_B_BIT) | (pinval&HALL_C_BIT) )>>24 ;// read hall a b c
    if(get_motorState()==true)
    {
      PID_timeout_count=0;
    }
    switch (hall_val)
    {
      case (0b101):  
      {
        hall_eintc_l();
        hall_eintb_h();
        hall_einta_l();
        if (get_motorState()==true)
        {
          P1CLR = PH_A_HIGH_BIT|PH_A_LOW_BIT|PH_B_HIGH_BIT|PH_C_LOW_BIT;
          delay_us(2);
          P1SET = PH_C_HIGH_BIT | PH_B_LOW_BIT;
        }
        else
         P1CLR = PH_A_HIGH_BIT|PH_A_LOW_BIT|PH_B_HIGH_BIT|PH_B_LOW_BIT|PH_C_HIGH_BIT|PH_C_LOW_BIT; // TURN OFF MOTOR
      }
      break;
      case (0b001):  
      {
        hall_eintc_h();
        hall_eintb_h();
        hall_einta_l();
        
        if (get_motorState()==true)
        {
          P1CLR = PH_A_LOW_BIT|PH_B_HIGH_BIT|PH_C_HIGH_BIT|PH_C_LOW_BIT;
          delay_us(2);
          P1SET = PH_A_HIGH_BIT | PH_B_LOW_BIT;
        }
        else
         P1CLR = PH_A_HIGH_BIT|PH_A_LOW_BIT|PH_B_HIGH_BIT|PH_B_LOW_BIT|PH_C_HIGH_BIT|PH_C_LOW_BIT; // TURN OFF MOTOR
      }
      break;
      case (0b011):  
      {
        hall_eintc_h();
        hall_eintb_l();
        hall_einta_l();
        
        if (get_motorState()==true)
        {
          P1CLR = PH_A_LOW_BIT|PH_B_HIGH_BIT|PH_B_LOW_BIT|PH_C_HIGH_BIT;
          delay_us(2);
          P1SET = PH_A_HIGH_BIT | PH_C_LOW_BIT;
        }
        else
         P1CLR = PH_A_HIGH_BIT|PH_A_LOW_BIT|PH_B_HIGH_BIT|PH_B_LOW_BIT|PH_C_HIGH_BIT|PH_C_LOW_BIT; // TURN OFF MOTOR
      }
      break;
      case (0b010):  
      {
        hall_eintc_h();
        hall_eintb_l();
        hall_einta_h();
        
        if (get_motorState()==true)
        {
          P1CLR = PH_A_HIGH_BIT|PH_A_LOW_BIT|PH_B_LOW_BIT|PH_C_HIGH_BIT;
          delay_us(2);
          P1SET = PH_B_HIGH_BIT | PH_C_LOW_BIT;
        }
        else
         P1CLR = PH_A_HIGH_BIT|PH_A_LOW_BIT|PH_B_HIGH_BIT|PH_B_LOW_BIT|PH_C_HIGH_BIT|PH_C_LOW_BIT; // TURN OFF MOTOR
      }
      break;
      case (0b110):  
      {
        hall_eintc_l();
        hall_eintb_l();
        hall_einta_h();
        if (get_motorState()==true)
        {
          P1CLR = PH_A_HIGH_BIT|PH_B_LOW_BIT|PH_C_HIGH_BIT|PH_C_LOW_BIT;
          delay_us(2);
          P1SET = PH_B_HIGH_BIT | PH_A_LOW_BIT;
        }
        else
         P1CLR = PH_A_HIGH_BIT|PH_A_LOW_BIT|PH_B_HIGH_BIT|PH_B_LOW_BIT|PH_C_HIGH_BIT|PH_C_LOW_BIT; // TURN OFF MOTOR
      }
      break;
      case (0b100):  
      {
        hall_eintc_l();
        hall_eintb_h();
        hall_einta_h();
        if (get_motorState()==true)
        {
          P1CLR = PH_A_HIGH_BIT|PH_B_HIGH_BIT|PH_B_LOW_BIT|PH_C_LOW_BIT;
          delay_us(2);
          P1SET = PH_C_HIGH_BIT | PH_A_LOW_BIT;
        }
        else
         P1CLR = PH_A_HIGH_BIT|PH_A_LOW_BIT|PH_B_HIGH_BIT|PH_B_LOW_BIT|PH_C_HIGH_BIT|PH_C_LOW_BIT; // TURN OFF MOTOR
      }
      break;
      default:
      {
         P1CLR = PH_A_HIGH_BIT|PH_A_LOW_BIT|PH_B_HIGH_BIT|PH_B_LOW_BIT|PH_C_HIGH_BIT|PH_C_LOW_BIT; // TURN OFF MOTOR
      }
      break;
    }
  EXTINT |= HALL_A_INT_FLAG|HALL_B_INT_FLAG|HALL_C_INT_FLAG;
  VICVectAddr = 0x00;
}

extern "C" void uart_init (void)
{
 PINSEL0|= UART_PINSEL0_TXD0|UART_PINSEL0_RXD0|UART_PINSEL0_TXD1|UART_PINSEL0_RXD1; 
 U0LCR |= 0B11;    //8 BIT CHAR  1 STOP BIT NO PARITY
 U0LCR |= 1<<7;    // DLAB=1
 U0DLL = 74;
 U0DLM = 0;
 U0FDR = 7 | (12<<4);   //BAUD RATE 38400
 U0LCR &= ~(1<<7);   //DLAB =0
 U0FCR |= 1|(1<<1)|(1<<2); // ENABLE AND CLEAR FIFO
 U0TER = 1<<7;  //ENABLE TRANSMIT
// U0IER |= 1<<0; //RDA INTERRUPT ENABLE

 U1LCR |= 0B11;    //8 BIT CHAR  1 STOP BIT NO PARITY
 U1LCR |= 1<<7;    // DLAB=1

 U1DLL = 74;
 U1DLM = 0;
 U1FDR = 7 | (12<<4);   //BAUD RATE 38400
// U1DLL = 170;
// U1DLM = 1;
// U1FDR = 1 | (10<<4);   //BAUD RATE 9600
 U1LCR &= ~(1<<7);   //DLAB =0
 U1FCR |= 1|(1<<1)|(1<<2); // ENABLE AND CLEAR FIFO
 U1TER = 1<<7;  //ENABLE TRANSMIT
 U1IER |= 1<<0; //RDA INTERRUPT ENABLE


 // ISR SETTINGS
 VICIntSelect &= ~( 1<<6 );  //UART0 
 VICVectCntl0 = 0x20 | 6;    //UART0
 VICVectCntl3 = 0x20 | 7;    //UART1
// VICVectAddr0 = (unsigned int)uart0ISR;
 VICVectAddr3 = (unsigned int)uart1ISR;
 VICIntEnable |= ( 1<<6 )|(1<<7);   // ISR ENABLE

}

extern "C" void uart0_tx_array (char* data, char length)
{ 
  int i=0;
  for (i=0;i<length;i++)
  {
    uart0_tx(data[i]); 
    vTaskDelay(2);
  }
}

extern "C" void uart1_tx_array (char* data, char length)
{ 
  int i=0;
  for (i=0;i<length;i++)
    {
      uart1_tx(data[i]); 
      vTaskDelay(2);
    }
}

extern "C" void uart0_tx (char data)
{
  while(U0LSR|(1<<5)) //THR IS EMPTY
  {
    U0THR=data;
    break;
  }
}

extern "C" void uart1_tx (char data)
{
while(U1LSR|(1<<5)) //THR IS EMPTY
  {
    U1THR=data;
    break;
  }
}

extern "C" void initTimer1( void )
{
    PINSEL1 |= (3<<10);           // enable cap1.3 on p0.21 (pin P6 on logomatic v2)

    T1TCR = (1<<1);                             // disable and reset Timer0
    T1CTCR = (3<<2)|(3<<0);                     // count using CAP1.3 as the counter
    T1TCR = 1;                                  // enable the timer
}
inline char UA_decodeButtonState(void)
{
  return (upper_data[12]&(1<<7)); //((upper_data[12]&(1<<7))>>7);
}
inline char UA_decodeNewDriveMode(void)
{
  return ((upper_data[12]&(1<<6))>>6);
}
inline bool UA_decodeUpperTransient(void)
{
  return ((upper_data[12]&(1<<4)));//>>4);
}
inline bool UA_getUpperTransient(void)
{
  return (UA_upperTransientEvent);//>>4);
}
inline bool UA_decodeOnOffMode(void)
{
  return ((upper_data[12]&(1<<6))>>6);
}
int UA_getTargetButtonState(void)
{
  int state = 0;

  if(UA_decodeButtonState())
  {
    state = SWITCH_PRESS;
  }
  else
  {
    state = SWITCH_RELEASE;
  }
  return state;
}
void UA_checkForUpperTransient(void)
{
  if (UA_decodeUpperTransient())// == true)
  {
    UA_upperTransientEvent = true;
    UA_upperTransientEventLockout = true;
    UA_upperTransientEventLockoutTimer = 0;
  }
  else
  {
      if (UA_upperTransientEventLockout == false)
      {
          UA_upperTransientEvent = false;
      }
      else if (UA_upperTransientEventLockoutTimer++ == 10)
      {
        UA_upperTransientEventLockoutTimer = 0;
        UA_upperTransientEventLockout = false;
      }
      else
      { 
 //        buzzer_loadDoubleBeep();//buzzer_loadSingleBeep();//buzzer_loadDoubleBeep();
      }
//    buzzer(1);
//    return UA_upperTransientEvent;
  }
}
void UA_checkNewDriveMode(void)
{
  char new_drive_mode = UA_decodeNewDriveMode(); 
  if(new_drive_mode != UA_drive_mode)
  {
    if(new_drive_mode == AUTO_MODE)
    {
      UA_drive_mode = AUTO_MODE;
      button_targetState = SWITCH_PRESS;
      buzzer_loadSingleBeep();
    }
    else if(new_drive_mode == MANUAL_MODE)
    {
      UA_drive_mode = MANUAL_MODE;
      button_targetState = SWITCH_PRESS;
      buzzer_loadDoubleBeep();
    }
    else if(new_drive_mode == OFF_MODE)
    {
      UA_drive_mode = OFF_MODE;
      buzzer_loadDoubleBeep();
 //     buzzer_loadNoBeep();
    } 
  }
}
void UA_updateButtonPressedCounter(void)
{
  UA_pressed_delay++;
}
void UA_resetButtonPressedCounter(void)
{
  UA_pressed_delay = 0;
}
inline int UA_getButtonPressedCounter(void)
{
  return UA_pressed_delay;
}
void UA_transPressedToReleased(void)
{
  UA_pressed_delay=0;
  UA_cruse= 1;
  if((UA_cruse_speed - CT_get_speed() >0.3))
  {
    CT_resetSpeedSetpoint();
  }
  else
  {
      CT_setSpeedSetpoint(PWMMR4/PID_CONSTANT/PID_RATIO_CONSTANT);
  }  
}
void UA_button_stateMachine(void)
{
  #define SWITCH_PRESSED  0
  #define SWITCH_RELEASED 1
  static int button_state       = SWITCH_RELEASED;
  
  switch (button_state)
  {
    case (SWITCH_RELEASED):
      if (button_targetState == SWITCH_PRESS) // switch pressed
      {
        UA_resetButtonPressedCounter();
        UA_updateButtonPressedCounter();
        new_press=1;
 //       UA_pressed_delay = 1;
        button_state = SWITCH_PRESSED;
        break;
      }
      UA_resetButtonPressedCounter();
//      UA_pressed_delay = 0;
    break;
    case (SWITCH_PRESSED):
    {
      if (button_targetState==SWITCH_RELEASE) // releasing
      {   
        UA_resetButtonPressedCounter();
        UA_transPressedToReleased();
//        UA_pressed_delay=0;
        button_state = SWITCH_RELEASED;
        break;
      }
      UA_updateButtonPressedCounter();
 //     UA_pressed_delay++;
    }
    break;
  }
}
void UA_drive_stateMachine(void)
{
  switch (UA_drive_mode)
  {
    case (AUTO_MODE):
    break;
    case (MANUAL_MODE):
    break;
    case (OFF_MODE):
    break;
  }
}
void UA_readSwitches( void )
{
    button_targetState = UA_getTargetButtonState();
    UA_button_stateMachine();
    
    UA_checkNewDriveMode();
    UA_drive_stateMachine();

    UA_checkForUpperTransient();
    PID_checkForLowerTransient();
}
void CountingTask( void * pvParameters )
{
    sTaskParams_t *  p_Args = reinterpret_cast<sTaskParams_t *>(pvParameters);

    CT_kinematic_init();
    float temp_filt_speed = 0.0f;

    while(true)
    {         
//        P1CLR |= TDI;// | TDO | TCK | TMS; 
        CT_kinematic_filter_prep();
        CT_get_encoder_SpeedAndDirection(); 
        CT_kinematic_filter_update();
        CT_set_speed(p_Args->frequency, CT_get_cap_val());
//        temp_speed = CT_butterworth_filter_update(CT_get_cap_val());
        temp_filt_speed = CT_average_filter_update(CT_get_cap_val());
        CT_set_filter_speed(p_Args->frequency, temp_filt_speed);
//    CT_speed        = ( (float)CT_cap_val  * ((float)p_Args->frequency) * (WHEEL_RADIUS) ) / ( TICKS_PER_REVOLUTION*((float)2.801127) );
//    CT_filter_speed  = ( (float)filter_y * ((float)p_Args->frequency) * (WHEEL_RADIUS) ) / ( TICKS_PER_REVOLUTION*((float)2.801127) ); 
       
        if ( UA_drive_mode == AUTO_MODE )
        {
            CT_pushMode_main();
        }
        else if ( UA_drive_mode == OFF_MODE )
        {
            CT_resetNewSpeedSetpoint();
            CT_resetSpeedSetpoint();
            CT_setPushDetected(false);
//            CT_delay_after_manual_mode = 0;   // inserting this line causes the motor to not shut off after hitting the button to go from AUTO to MAN (double beep) it shuts off after hitting the button again and it goes back to AUTO
        }       
        vTaskDelay(configTICK_RATE_HZ / (p_Args->frequency));
 //      vTaskDelay(0.5*configTICK_RATE_HZ / (p_Args->frequency));
    }
}

void Timer_1msTask_helper_WDT(void)
{
  static unsigned int timer = 0;
 // WDT_service();
  if (++timer == 1000)
  {
//    buzzer(0);
    timer = 0;
    WDFEED = 0x00;
  }
}
void Timer_1msTask_rampUp_recovery(void)
{
  set_pwm_FORCED(40);
  motorShutDownRecovery = false;
}
void Timer_1msTask_helper(void)
{
   update_pwm();
}
void Timer_1msTask(void *pvParameters)
{
  sTaskParams_t *  p_Args = reinterpret_cast<sTaskParams_t *>(pvParameters);
  WDT_init();
  while(1) 
  {
    motorPhasesConfig();
    motorHallSensConfig();
    pwm_config();

    WDT_service();
    Timer_1msTask_helper();
   vTaskDelay(configTICK_RATE_HZ / (p_Args->frequency));
  }
}
void buzzer (char buzz)
{
  if (buzz==BUZZER_ON)
    P0SET = 1<<12;
  else
    P0CLR  = 1<<12; 
}
void buzzer_loadSingleBeep(void)
{
  Buzzer_delay1 = 2;  // single beep
  Buzzer_delay2 = 0;
}
void buzzer_loadDoubleBeep(void)
{
  Buzzer_delay1 = 0; // double beep
  Buzzer_delay2 = 3;
}
void buzzer_loadNoBeep(void)
{
  Buzzer_delay1 = 0; 
  Buzzer_delay2 = 0;
}
void buzzer_logic(void)
{
  if(Buzzer_delay2>0)
  {
    if(Buzzer_delay2==2)
    {
      buzzer(0);
    }
    else
    {
      buzzer(1);
    }
    Buzzer_delay2--;
    Buzzer_delay1=0;
  }
  else
  {
    if(Buzzer_delay1>0)
    {
      buzzer(1);
      Buzzer_delay1--;
      Buzzer_delay2=0;
    }
    else
    {
      buzzer(0);
    }
  }
}
void PID_pushMode_turnMotorOffReset(void)
{
    PID_xMode_turnMotorOff();
    CT_setPushDetected(false);
    CT_resetSpeedSetpoint();
    PID_stable=false;
}
void PID_pushMode_updatePreviousSpeedSetpoint(void)
{ 
  PID_prevSpeedSetpoint = CT_getSpeedSetpoint();
  PID_previous_ratio  = PID_current_ratio;

  PID_duty_unstable = 0;
  PID_duty_stable=0;
  PID_output = 0;
  PID_stable=false;
  pid_delay=0;
 }
inline bool PID_pushMode_detectBrakeAfterPush(void)
{
  return (PID_current_ratio > 1.8f);
}
//inline bool PID_pushMode_isMaxAccelUnderThresh(void)
//{
//  return (CT_getLastAccel()<300);
//}
inline float PID_pushMode_getBrakeRatio(void)
{
  return ( (PID_current_brake_ratio < PID_speed_ratio) ? PID_current_brake_ratio:PID_speed_ratio);
}
inline bool PID_pushMode_detectBrakeWhenStable(void)
{
  return ( PID_current_ratio > (PID_pushMode_getBrakeRatio()*PID_BRAKE_PROP) );
}
inline bool PID_pushMode_detectPushInPush(void)
{
  // need 2 samples in a row to have higher speed
  return ((PID_current_ratio < (PID_speed_ratio*PID_PUSH_PROP)) && 
          (PID_previous_ratio < (PID_speed_ratio*PID_PUSH_PROP)));
}
inline bool PID_pushMode_detectValidPushInPush(void)
{
  return ((PID_pushMode_detectPushInPush()==true) && (CT_pushMode_isMaxAccelUnderThresh() == true) && (CT_getPushDetectedDelay()>10));
}
void PID_checkForLowerTransient(void)
{
  if (CT_getLowerTransient() == true)
  {
    if (PID_lowerTransientEventLockout == false)
    {
      CT_setLowerTransient(false);
    }
    else if (PID_lowerTransientEventLockoutTimer++ >= 10)
    {
      if (PID_lowerTransientEventSendLock == false)
      {
        PID_lowerTransientEventLockoutTimer = 0;
        PID_lowerTransientEventLockout = false;
        CT_setLowerTransient(false);
        PID_lowerTransientEventSendLock = false;
      }
    }
    else
    {
//      buzzer_loadSingleBeep();
    }
  }
  if (CT_getLowerTransientWheelie() == true)
  {
    if (PID_lowerTransientWheelieEventLockout == false)
    {
      CT_setLowerTransientWheelie(false);
    }
    else if (PID_lowerTransientWheelieEventLockoutTimer++ >= 10)
    {
      if (PID_lowerTransientWheelieEventSendLock == false)
      {
        PID_lowerTransientWheelieEventLockoutTimer = 0;
        PID_lowerTransientWheelieEventLockout = false;
        CT_setLowerTransientWheelie(false);
        PID_lowerTransientWheelieEventSendLock = false;
      }
    }
    else
    {
//      buzzer_loadSingleBeep();
    }
  }       
}
void PID_pushMode_respondToBrake(void)
{
  PID_pushMode_turnMotorOffReset();
  CT_resetPushDetectedDelay();
}
void PID_pushMode_unstableSpeedControl(void * pidParameters)
{
  sPIDParams_t*  p_Args = reinterpret_cast<sPIDParams_t *>(pidParameters);
  PID_output = PID_RATIO_CONSTANT * CT_getSpeedSetpoint() ;
  PID_duty_unstable = (PID_output <10.0f) ? ((int) 0) : ((int) PID_output);
  if (PID_duty_unstable >100)
    PID_duty_unstable=100;
  set_pwm_FORCED(PID_duty_unstable);
  set_motorState(true);//motor_run=1;
  hallISR_handler();
  
  PID_speed_ratio = PID_current_ratio;
  PID_current_brake_ratio=PID_current_ratio;
  if ( PID_speed_ratio > 1.8f ) 
    PID_speed_ratio = 1.8f;     // PID_speed_ratio initial 
  if (PID_pushMode_detectBrakeAfterPush() == true)                          // brake after push 
  {
    PID_pushMode_respondToBrake();
  }
}
void PID_pushMode_stableSpeedControl(void * pidParameters)
{
  sPIDParams_t*  p_Args = reinterpret_cast<sPIDParams_t *>(pidParameters);
  if(PID_pushMode_detectValidPushInPush()==true)   // (PID_current_ratio-PID_previous_ratio)<-0.1f
  {
    CT_pushMode_initPushInPushSequence();
  }
  PID_previous_ratio = PID_current_ratio;
//  if ((PID_current_ratio > (PID_speed_ratio*PID_BRAKE_PROP)))   //*******brake 
  if(PID_pushMode_detectBrakeWhenStable()==true)   //*******brake when PID_stable
  {
     PID_pushMode_respondToBrake();
  }  
  else //*******************no brake detected
  {
    set_pwm_FORCED(PID_duty_stable);
    set_motorState(true);//motor_run=1;
    hallISR_handler();
  }
}
void PID_pushMode_speedControlSM(void * pidParameters)
{
  sPIDParams_t*  p_Args = reinterpret_cast<sPIDParams_t *>(pidParameters);
  #define UNSTABLE  5
  #define STABLE_1  1
  #define STABLE_2  2
  #define STABLE_3  3
  #define STABLE_SS 4
  static unsigned int state = UNSTABLE;
  
  float temp_speed = CT_get_speed();
//  PID_output = 20.0f * CT_speedSetpoint ;
  PID_current_ratio = (PID_output)/temp_speed/PID_RATIO_CONSTANT;
  
  if (!PID_stable)
  {
    state = UNSTABLE;
  }
  if(UA_cruse==1)
  {
    PID_pushMode_enterButtonCruise();
    state = STABLE_1;
  }
  pid_delay++;  
  switch (state)
  {
    case UNSTABLE:
    {
      PID_pushMode_unstableSpeedControl(p_Args);
      if(pid_delay == 4)
      {
        PID_duty_stable = PID_duty_unstable;
        PID_stable = true;  
        PID_pushMode_stableSpeedControl(p_Args);
        state = STABLE_1;
      }
    }
    break;
    case STABLE_1:
    {
      if(pid_delay == 8)  //** prop PID_stable delay 
      {
        PID_speed_ratio = PID_current_ratio;
        PID_current_brake_ratio=PID_current_ratio;
        if ( PID_speed_ratio > 1.5f ) 
          PID_speed_ratio = 1.5f;
        state = STABLE_2;
      }
      PID_pushMode_stableSpeedControl(p_Args);
    }
    break;
    case STABLE_2:
    {
      if(pid_delay==12)  //** prop PID_stable delay 
      {
        PID_current_brake_ratio=PID_current_ratio;    
        state = STABLE_3;
      }
      PID_pushMode_stableSpeedControl(p_Args);
    }
    break;
    case STABLE_3:
    {
      if(pid_delay>=20)  //** prop PID_stable renew 
      {
        PID_speed_ratio = (PID_output)/CT_get_filter_speed()/PID_RATIO_CONSTANT;
        if ( PID_speed_ratio > 1.5f ) 
          PID_speed_ratio = 1.5f;
        state = STABLE_SS;
      }
      PID_pushMode_stableSpeedControl(p_Args);
    }
    break;
    case STABLE_SS:
    {
      PID_speed_ratio = (PID_output)/CT_get_filter_speed()/PID_RATIO_CONSTANT;
      if ( PID_speed_ratio > 1.5f ) 
        PID_speed_ratio = 1.5f;
      state = STABLE_SS;
      PID_pushMode_stableSpeedControl(p_Args);
    }
    break;
   }
}
void PID_pushMode_enterButtonCruise(void)
{
  UA_cruse=0;
  PID_prevSpeedSetpoint= CT_getSpeedSetpoint();
//          p_Args->setpoint=_speed;
  PID_stable=true;
  pid_delay=7;

  PID_output = PID_RATIO_CONSTANT * CT_getSpeedSetpoint() ;
  PID_duty_unstable = (PID_output <10.0f) ? ((int) 0) : ((int) PID_output);
  if (PID_duty_unstable >100)
    PID_duty_unstable=100;
  set_pwm_FORCED(PID_duty_unstable);
  set_motorState(true);//motor_run=1;
  hallISR_handler();
//          set_pwm_FORCED(PWMMR4/40);
//          motor_run=1;
//          hallISR_handler();
  PID_current_ratio = (PID_output)/CT_get_speed()/PID_RATIO_CONSTANT;
  PID_current_brake_ratio = PID_current_ratio;
  PID_duty_stable = PID_duty_unstable;
}
void PID_pushMode_motorRunning(void * pidParameters)
{
    sPIDParams_t*  p_Args = reinterpret_cast<sPIDParams_t *>(pidParameters);
    
    PID_manual_output=0;
//    press_speed=0;
    if ( PID_prevSpeedSetpoint != CT_getSpeedSetpoint() ) //***** new setpoint, reset all variables
    {
      // prevSp = sp, PID_duty_unstable = 0, PID_output = 0, PID_previous_ratio = PID_current_ratio, PID_stable = false, pid_delay = 0, PID_duty_stable = 0
      // this will keep happening until we see a speed decrease
      PID_pushMode_updatePreviousSpeedSetpoint();   
    }
    
    if ( CT_getSpeedSetpoint() > 0.0f )        //***** CT_speedSetpoint>0 , motor start
    {
      PID_pushMode_speedControlSM(p_Args);
 //       PID_pushMode_speedControlOld(p_Args);
    }
    else  // CT_speedSetpoint==0, then motor stop
    {
      PID_xMode_turnMotorOff();
    }
  }
void PID_manualMode_runMotorMaxSpeed(float temp_spd)
{
  set_motorState(true);//motor_run=1;
  hallISR_handler();
//  float temp_spd = CT_get_speed();
  PID_manual_output=(int)(temp_spd*17);
  set_pwm_button(PID_manual_output);
}
void PID_manualMode_runMotorButtonOn(void)
{
  PID_manual_output=100;  
  set_motorState(true);//motor_run=1;
  hallISR_handler();
  set_pwm(PID_manual_output);
}
void PID_xMode_turnMotorOff(void)
{
  PID_manual_output=0;  
  set_motorState(false);//motor_run=1;
  hallISR_handler();
  set_pwm(PID_manual_output);
}
void PID_manualMode_stateMachine(void)
{
  #define MANUAL_OFF 0
  #define MANUAL_BUTTON_ON 1
  #define MANUAL_CRUISE 2
  #define MANUAL_MAX_SPEED 3
  static char PID_stateManualSM = MANUAL_OFF;
  static float target_spd=0;

  CT_pushMode_disable();

  int countsButtonPressed = UA_getButtonPressedCounter();
  
//   if (countsButtonPressed == 1) // button was just pressed
//   {
//    PID_stateManualSM = MANUAL_OFF;
//   }
//   if ((get_motorState()==true) && (CT_get_speed() > 1.5f))
//   {
//      PID_stateManualSM = MANUAL_MAX_SPEED; 
//   }

//   if(countsButtonPressed <= (3*4))
//   {
//      if ((get_motorState()==true)|(CT_get_speed()<1.5f) )     //turn off motor if motor is on
//      {
//        PID_stateManualSM = MANUAL_OFF;
//        PID_xMode_turnMotorOff();
//      }
//      else                            
//      {
//        PID_stateManualSM = MANUAL_MAX_SPEED;
//        countsButtonPressed=3*4+1;
//      }
//   }
  if(new_press==1)  // on entry 
  {
    new_press=0;
    if(get_motorState()==true)
    {
      PID_stateManualSM = MANUAL_OFF;
    }
    else
    {
      if(CT_get_speed()<1.5f)
      {
        PID_stateManualSM = MANUAL_OFF;//MANUAL_BUTTON_ON;
      }
      else
      {
        PID_stateManualSM = MANUAL_MAX_SPEED;
        target_spd=CT_get_speed();
      }
    }
  }
  switch (PID_stateManualSM)
    {
      case (MANUAL_OFF):                   //motor off
        if(countsButtonPressed>(3*4))
        {
          if(CT_get_speed()>1.5f)             //speed limit
          {    
            PID_stateManualSM = MANUAL_MAX_SPEED;
            target_spd=CT_get_speed();
            PID_manualMode_runMotorMaxSpeed(target_spd); 
            break;
          }
          PID_stateManualSM = MANUAL_BUTTON_ON;
          PID_manualMode_runMotorButtonOn();
          break;
        }
        PID_xMode_turnMotorOff();
      break;
      case (MANUAL_BUTTON_ON):            //slow ramp
//        if (quickPress == true)
//        {
//          PID_stateManualSM = MANUAL_CRUISE;
//          quickPress = false;
//          break;
//        }
//        if (countsButtonPressed <= 0)
//        {
//          PID_stateManualSM = MANUAL_OFF;
//          PID_xMode_turnMotorOff();
//          break;
//        }
//        if(CT_get_speed()>1.5f)             //speed limit
//        {    
//          PID_stateManualSM = MANUAL_MAX_SPEED;
//          PID_manualMode_runMotorMaxSpeed();  
//          break;
//        }
        PID_manualMode_runMotorButtonOn();     
      break;
      case (MANUAL_MAX_SPEED):
//        if (countsButtonPressed <= 0)
//        {
//          PID_stateManualSM = MANUAL_OFF;
//          PID_xMode_turnMotorOff();
//          break;
//        }
//        if (CT_get_speed() <= 1.5f)
//        {
//          PID_stateManualSM = MANUAL_BUTTON_ON;
//          PID_manualMode_runMotorButtonOn();
//          break;
//        }
        PID_manualMode_runMotorMaxSpeed(target_spd);  
      break;
    }
}
void PID_manualMode_motorRunning(void * pvParameters)
{
    sPIDParams_t*  p_Args = reinterpret_cast<sPIDParams_t *>(pvParameters);
//    CT_push_detected = false;
//    CT_resetSpeedSetpoint();
//    CT_delay_after_manual_mode = 0;
  
    hallISR_handler();
    PID_manualMode_stateMachine();   
    //  speed_prop_sw= manual_output/p_Args->Ki/CT_get_speed();	  
    UA_cruse_speed=CT_get_speed();
}
void PID_timeOut_logic(void * pvParameters)
{
  sPIDParams_t*  p_Args = reinterpret_cast<sPIDParams_t *>(pvParameters);
  if(PID_timeout_count>(300*(p_Args->frequency)))
  {
    PID_timeout_count=0;
    P0SET = L_POWER_BIT;
    delay_ms(1);
    P0CLR = L_POWER_BIT;
  }
  else
  {
    PID_timeout_count++;
  }
}
void PID_setGyroTransient(void)
{
    CT_setLowerTransient(true);
    PID_lowerTransientEventLockout = true;
    PID_lowerTransientEventSendLock = true;
    PID_lowerTransientEventLockoutTimer = 0;
///#     buzzer_loadSingleBeep();
//    P0CLR |= 1<<RED_LED;
}
void PID_setGyroWheelieTransient(void)
{
    CT_setLowerTransientWheelie(true);
    PID_lowerTransientWheelieEventLockout = true;
    PID_lowerTransientWheelieEventSendLock = true;
    PID_lowerTransientWheelieEventLockoutTimer = 0;
//     buzzer_loadSingleBeep();
//    P0CLR |= 1<<RED_LED;
}
void PID_lowerTransient(void)
{

  gyro_buff[gyro_buff_index]=gyro_data[0]+gyro_data[1]*256;
  
  gyro_buffSum=0;
  gyro_posSum = 0;
  gyro_negSum = 0;
  gyro_buffMin = gyro_buff[gyro_buff_index];
  gyro_buffMax = gyro_buff[gyro_buff_index];

  for (int i =0;i<GYRO_BUFF_SIZE;i++)
  {
     gyro_buffSum = gyro_buffSum + gyro_buff[i];   
     if (gyro_buff[i] > gyro_buffMax)
        gyro_buffMax = gyro_buff[i];
     if (gyro_buff[i] < gyro_buffMin)
        gyro_buffMin = gyro_buff[i];
     if (gyro_buff[i] >= 0)
        gyro_posSum += gyro_buff[i];
     if (gyro_buff[i] < 0)
        gyro_negSum += gyro_buff[i];
    }
    gyro_buffDiff = gyro_buffMax - gyro_buffMin;
    gyro_absSum = gyro_posSum + gyro_negSum*(-1);

    if ((gyro_buffDiff > GYRO_DIFF_THRESH) &&
       ((gyro_buffMax > GYRO_MAX_THRESH_2) || (gyro_buffMin < GYRO_MIN_THRESH_2)) &&
       ((gyro_buffMax > GYRO_MAX_THRESH_1) && (gyro_buffMin < GYRO_MIN_THRESH_1)))
    {
        PID_setGyroTransient();
    }

    else if (gyro_buffSum < GYRO_NEG_SUM_THRESH)
  {
//    PID_setGyroTransient();
    
  }
  if ((gyro_posSum > 15000)&&(gyro_absSum > 10000)&&(CT_get_speed() < 0.5))
  {
    PID_setGyroWheelieTransient();
  }

  if (++gyro_buff_index >= GYRO_BUFF_SIZE)
      gyro_buff_index = 0;
}
void debugDataStream_init(void)
{
    float acc_x=0.0f;
    //    upper_data=0;
    i2c_acc_init();
    vTaskDelay(20);
    i2c_gyro_init();
}
void PID_battCurrentFilter(void) 
{
  if ((PID_battCurrent_new > PID_battCurrent)&&((PID_battCurrent_new - PID_battCurrent)> 15))
  {
    ++PID_battChallenger;
    if (PID_battChallenger > 10)
    {
      PID_battChallenger = 0;
      PID_battCurrent = (PID_battCurrent_new>>2) + 3*(PID_battCurrent>>2);
    }
    else
    {
      PID_battCurrent = (PID_battCurrent>>2) + 3*(PID_battCurrent>>2);
    }
  }
  else
  {
    PID_battChallenger = 0;
  }
}
void debugDataStream_transmit(void * pvParameters)
{
  sPIDParams_t*  p_Args = reinterpret_cast<sPIDParams_t *>(pvParameters);
  
  get_gyro_data();
  vTaskDelay(20);
  get_acc_data();
  vTaskDelay(20);
  
  PID_lowerTransient();

  PID_temperature_new = adc_get(&AD0CR, AD_CR_SEL4, &AD0DR4);//getADC1();
  PID_temperature_new = (PID_temperature_new>>00) & 0x3FF;
  PID_battCurrent_new = adc_get(&AD0CR, AD_CR_SEL1, &AD0DR1);
  PID_battCurrent_new = (PID_battCurrent_new>>00) & 0x3FF;

//      upper_data[0] = (char)((PID_temperature_new>>8)&0x3);
//      upper_data[1] = PID_temperature_new&0xFF;
  upper_data[0] = (char)((PID_battCurrent_new>>8)&0x3);
  upper_data[1] = PID_battCurrent_new&0xFF;

    PID_temperature = 7*(PID_temperature>>3) + 1*(PID_temperature_new>>3);
    PID_battCurrent = 7*(PID_battCurrent>>3) + 1*(PID_battCurrent_new>>3);
//  PID_battCurrentFilter();
  
  //upper_data[0] and upper_data[1] are upper accel x
  upper_data[2] = (char)((PID_battCurrent>>8)&0x3);
  upper_data[3] = PID_battCurrent&0xFF;
  //upper_data[4] and upper_data[5] are battery voltage
  if (CT_getLowerTransient() == true)
  {
      upper_data[12] |= (1<<1);
      PID_lowerTransientEventSendLock = false;
  }
  else
  {
    upper_data[12] &= ~(1<<1);
  }
  if (CT_getLowerTransientWheelie() == true)
  {
      upper_data[12] |= (1<<2);
      PID_lowerTransientWheelieEventSendLock = false;
  }
  else
  {
    upper_data[12] &= ~(1<<2);
  }

  accel_data[0] = (char)((PID_temperature_new>>8)&0x3);
  accel_data[1] = PID_temperature_new&0xFF;
  
  uart0_tx(0xff);
  uart0_tx(0x00);
  uart0_tx((char) (CT_getSpeedSetpoint() * 10));
  uart0_tx((char) (CT_get_speed() * 10));

  vTaskDelay(2);
  uart0_tx_array(upper_data,13);

  uart0_tx_array(gyro_data,6);

  uart0_tx_array(accel_data,6);
  vTaskDelay(2);
  uart0_tx(upper_data[12]&(1<<4));//max_diff);
  uart0_tx(CT_getLastAccel());
  vTaskDelay(2);
  uart0_tx((char) ((CT_get_filter_speed()+10.0f) * 10));
}
void PIDTask( void * pvParameters )
{
    sPIDParams_t *  p_Args = reinterpret_cast<sPIDParams_t *>(pvParameters);

 //   char press_speed = 0;    //when press button with speed >1mph, use high acc
    debugDataStream_init();
    for (int i =0;i<GYRO_BUFF_SIZE;i++)
    {
        gyro_buff[i]=0;
    }
    
    vTaskDelay(20);
//    vTaskDelay(1);
    set_pwm(0);
    
    set_motorState(false);
    delay_ms(1);
    
    while(true)
    {  
      debugDataStream_transmit(p_Args);
         
      PID_timeOut_logic(p_Args);
//   drive mode change        

      if (PID_battCurrent > 250) 
      {
        P0SET |= FAN_BIT;
      }
      else
      {
        P0CLR |= FAN_BIT;
      }
      buzzer_logic();
      if (PID_temperature < 100)
      { 
        PID_pushMode_turnMotorOffReset();
      }
      else if ( UA_drive_mode != MANUAL_MODE)//OFF_MODE )
      {       
//        buzzer_logic();
        if ( button_targetState ==SWITCH_PRESS ) //********** button pressed
//        if (UA_drive_mode == MANUAL_MODE)
        {
          PID_manualMode_motorRunning(p_Args); 
        }
        else //if ( UA_drive_mode == AUTO_MODE )//******************* button not pressed
        {   
          PID_pushMode_motorRunning(p_Args);
        }   
      }
      else//************drive mode == off mode
      {
        PID_pushMode_turnMotorOffReset();
      }
//      P1SET |= TDO;//TDI | TDO | TCK | TMS;
      vTaskDelay(configTICK_RATE_HZ / (p_Args->frequency));
    }
}
void pulse_low(unsigned int _mode)
{
  P1CLR = PH_A_HIGH_BIT|PH_A_LOW_BIT|PH_B_HIGH_BIT|PH_B_LOW_BIT|PH_C_HIGH_BIT|PH_C_LOW_BIT;
  delay_ms(1);
  P1SET= PH_A_LOW_BIT;
  if (_mode == 1) {
    while(1);
  }
  else {
    delay_ms(1);
    P1CLR = PH_A_HIGH_BIT|PH_A_LOW_BIT|PH_B_HIGH_BIT|PH_B_LOW_BIT|PH_C_HIGH_BIT|PH_C_LOW_BIT;
    delay_ms(1);
    P1SET= PH_B_LOW_BIT;
    delay_ms(1);
    P1CLR = PH_A_HIGH_BIT|PH_A_LOW_BIT|PH_B_HIGH_BIT|PH_B_LOW_BIT|PH_C_HIGH_BIT|PH_C_LOW_BIT;
    delay_ms(1);
    P1SET= PH_C_LOW_BIT;
    delay_ms(1);
    P1CLR = PH_A_HIGH_BIT|PH_A_LOW_BIT|PH_B_HIGH_BIT|PH_B_LOW_BIT|PH_C_HIGH_BIT|PH_C_LOW_BIT;
    while(1);
  }
}
/*
  bool checkIfMotorIsMoving(void)
  Description: 
  Notes: This function is not currently used. It was intended to detect if the motor was moving on startup, back in the days when 
          we were having issues with the motor resetting. If the motor reset, the code would restart and would call this function to 
          see if the motor was already running. If it was running, then the startup code would continue running the motor without further
          delay. Otherwise, it would do a normal startup. This function was abandoned because it took too long (several hundred milli-sec) to detect 
          if the motor was running. In a reset condition, the motor would stop on the order of 10's of ms so this function didn't work! 
          We switched to using the encoder to detect if the motor was running which works much faster. 
*/
bool checkIfMotorIsMoving(void)
{
    #define NUM_LOOPS 100
    #define NUM_COMMUTATIONS 10
    int pinval = P1PIN;
    unsigned int numCommutationsSensed = 0;
    bool motorIsMoving = false;
    int hall_val=0;
    int hall_val_last = 0;

    hall_val = ( (pinval&HALL_A_BIT) | (pinval&HALL_B_BIT) | (pinval&HALL_C_BIT) )>>24 ;
 ///   buzzer(0);

    for(unsigned int i = 0; i < NUM_LOOPS; i++)
    {
        hall_val_last = hall_val;
        delay_ms(100);
        pinval = P1PIN;
        hall_val = ( (pinval&HALL_A_BIT) | (pinval&HALL_B_BIT) | (pinval&HALL_C_BIT) )>>24 ;
        if (hall_val_last!=hall_val)   // state change
        {
            if (++numCommutationsSensed >= NUM_COMMUTATIONS)
            {		
                motorIsMoving = true;
   //             buzzer(0);
            }
        }
    }
    return motorIsMoving;
}

inline void set_motorState(bool _state)
{
//  buzzer(1);
//  *((unsigned int *)&motorIsOn) = _state;
//  buzzer(0);
    motor_run = _state;
}
inline bool get_motorState(void)
{
  return motor_run;
}
void motorPhasesInit(void)
{
   P1DIR |=PH_A_HIGH_BIT|PH_A_LOW_BIT|PH_B_HIGH_BIT|PH_B_LOW_BIT|PH_C_HIGH_BIT|PH_C_LOW_BIT; // PID_output
   P1CLR = PH_A_HIGH_BIT|PH_A_LOW_BIT|PH_B_HIGH_BIT|PH_B_LOW_BIT|PH_C_HIGH_BIT|PH_C_LOW_BIT;
}
void motorPhasesConfig(void)
{
  P1DIR |=PH_A_HIGH_BIT|PH_A_LOW_BIT|PH_B_HIGH_BIT|PH_B_LOW_BIT|PH_C_HIGH_BIT|PH_C_LOW_BIT;
}
void motorPhasesClearAll(void)
{
  P1CLR = PH_A_HIGH_BIT|PH_A_LOW_BIT|PH_B_HIGH_BIT|PH_B_LOW_BIT|PH_C_HIGH_BIT|PH_C_LOW_BIT;
}
void motorHallSensConfig(void)
{
   P1DIR &= ~(HALL_A_BIT|HALL_B_BIT|HALL_C_BIT);
}

int main( void )
{
   // Initialize the logomatic board processor and preipherals
   bootup();
    initTimer1();
   
 adc_init(&PINSEL1, &AD0CR, PINSEL1_P0p25_MASK, PINSEL1_P0p25_ADC0p4, PCONP_ADC0_PWR, (AD_CR_PDN_ON | AD_CR_CLK_DIV_11 | AD0CR_SEL_AD0p4)); // temp
 adc_init(&PINSEL1, &AD0CR, PINSEL1_P0p28_MASK, PINSEL1_P0p28_ADC0p1, PCONP_ADC0_PWR, (AD_CR_PDN_ON | AD_CR_CLK_DIV_11 | AD0CR_SEL_AD0p1)); // current
 
  motorPhasesInit();
  motorHallSensConfig();
  pwm_init();

  int direction = 0;
  int timerval = 0;
//  vTaskDelay(configTICK_RATE_HZ / 670); // 1.5ms
  delay_ms(1);
  delay_us(500);
 
      delay_ms(100);

        Buzzer_delay2=3;
        Buzzer_delay1=0;


   hallISR_init();

  for (int i=20;i>0;i--)
  {
    P1SET= PH_A_LOW_BIT;
    delay_us(5);
    P1CLR= PH_A_LOW_BIT;
    delay_us(5);
   } 
  for (int i=20;i>0;i--)
  {
    P1SET= PH_B_LOW_BIT;
    delay_us(5);
    P1CLR= PH_B_LOW_BIT;
    delay_us(5);
   } 
  for (int i=20;i>0;i--)
  {
    P1SET= PH_C_LOW_BIT;
    delay_us(5);
    P1CLR= PH_C_LOW_BIT;
    delay_us(5);
   } 
   motorPhasesClearAll();

   uart_init();
   i2c_init();

   delay_ms(100);
   P0DIR |= POWER_ON_BIT | L_POWER_BIT | FAN_BIT;// PID_output
   P0SET = POWER_ON_BIT;
   P0CLR = L_POWER_BIT | FAN_BIT;

   xTaskCreate(CountingTask,
               reinterpret_cast<const signed char *>("TCK"),
               TASK_STACK_SIZE,
               &CountingArgs,
               COUNTING_TASK_PRIORITY,
               &xCountingTaskHandle
               );

  xTaskCreate(Timer_1msTask,
               reinterpret_cast<const signed char *>("Timer1ms"),
               TASK_STACK_SIZE,
               &Timer1msArgs,
               Timer_1ms_TASK_PRIORITY,
               &xTimer_1msHandle
             );
   
   // Create the PID controller task
   xTaskCreate(PIDTask,
               reinterpret_cast<const signed char *>("PID"),
               TASK_STACK_SIZE,
               &PIDArgs,
               PID_TASK_PRIORITY,
               &xPIDTaskHandle
               );

   // Start the multitasking environment
   vTaskStartScheduler();
   return 0;
} // main

static void I2CISR (void) __attribute__ ((naked));
static void I2CISR (void)
{
  portSAVE_CONTEXT ();
  I2CISR_handler();
  portRESTORE_CONTEXT();
}

char acc_test(void)
{
  return accel_data[1];
}
float acc_cal_x(void)
{
  acc_x=(  accel_data[0]&0b00001111+accel_data[1]*16 )/4096.0 *4.0-2;
  return acc_x;
}
float acc_cal_y(void)
{
  acc_y=(  accel_data[2]&0b00001111+accel_data[3]*16 )/4096.0 *4.0-2;
  return acc_y;
}
float acc_cal_z(void)
{
  acc_z=(  accel_data[4]&0b00001111+accel_data[5]*16 )/4096.0 *4.0-2;
  return acc_z;
}

void i2c_init(void)
{
  PINSEL0=(PINSEL0 & 0xffffff0f) | 0x50;
  I20SCLH = 200;
  I20SCLL = 200;     //PCLK=12MX6    FRQ=PCLK/(100+100)=360KHZ
 // ISR SETTINGS
   VICIntSelect = 0x00;  //I2C0 
   VICVectCntl4 = 0x20 | 9;    //I2C0
   VICVectAddr4 = ( long )I2CISR;
   VICIntEnable |= ( 1<<9 );   // ISR ENABLE
   VICDefVectAddr=( long )I2CISR;
   PCONP|=1<<7;// I2C POWER ENABLE
   I20ADR=0X72;
   I20CONCLR = 0x6c;
   I20CONSET = 0x40;  //ENABLE
   i2c_bus_busy=0; // i2c bus is not busy
}

void i2c_acc_init(void)
{
  int _temp_i=0;
  i2c_bus_busy=1;    // i2c bus is busy
  I2CAddress = ACCEL_ADDR;
  I2CData = ACCEL_INIT_MASK;
  I2C_reg_Address=CTRL_REG1;
  write_count=1;
//  i2cact=write;
  multi_read=0;
  I20CONCLR = 0x6c;
  I20CONSET = 0x40;  //ENABLE
  I20CONSET = 0x64; //start
}

void i2c_gyro_init(void)
{
  int _temp_i=0;
  i2c_bus_busy=1;    // i2c bus is busy
  I2CAddress = GYRO_ADDR;
  I2CData = 0x0F;
  I2C_reg_Address=0x20;
  write_count=1;

  multi_read=0;
  I20CONCLR = 0x6c;
  I20CONSET = 0x40;  //ENABLE
  I20CONSET = 0x64; //start
}

void get_acc_data(void)
{
  I2CAddress = ACCEL_ADDR;
  I2C_reg_Address = OUT_X_LSB;
  i2cact=READ_ACCEL;
  multi_read=1;
  read_count=0;

  I20CONCLR = 0X6C;
  I20CONSET = 0x00000040;   //enable
  I20CONSET = 0x00000020;   //start
}
void get_gyro_data(void)
{
  I2CAddress = GYRO_ADDR;
  I2C_reg_Address =OUT_X_GYRO ;
  i2cact=READ_GYRO;
  multi_read=1;
  read_count=0;

  I20CONCLR = 0X6C;
  I20CONSET = 0x00000040;   //enable
  I20CONSET = 0x00000020;   //start
}



void I2CISR_handler (void)
{
#define I2C_ACK_START     0x08
#define I2C_ACK_RPT_START 0x10
#define I2C_ACK_tbd1      0x18
#define I2C_ACK_tbd2      0x20
#define I2C_ACK_tbd3      0x28
#define I2C_ACK_tbd4      0x40
#define I2C_ACK_tbd5      0x50
#define I2C_ACK_tbd6      0x58
  
  switch (I20STAT&0xf8)
  {
//    case (0x00):   //bus error
//      I20CONSET =0X14;
//    break;

    case (0x08):   //start transmitted
      {
        I20DAT=I2CAddress; 
        I20CONCLR=0X20;  //CLEAR Sta
      }
     break;

    case (0x10):  // repeat start transmitted
      I20DAT=I2CAddress|1; ;  //read
      I20CONCLR=0x20;  // clear start & SI
    break;
//
    case (0x18):
      {
        I20DAT=I2C_reg_Address; 
      }
    break;
    
//    case (0x20):
//   // I20DAT = I2CAddress;
////    accel_data[1]=0x23;
//    
//    break;
    
    case (0x28):
      if(multi_read==0)   //not multi read
      {
        if (write_count>0)
        {
          I20DAT=I2CData;    //write ACCEL_INIT_MASK to acc sensor
          write_count--;
        }
        else
        {
//        accel_data[1]=0x23;    
        I20CONSET = 0x10;  //stop will transmitted
        i2c_bus_busy=0;  // free i2c bus
        }
      }
      else
      {
        I20CONSET = 0x00000020; // repeat start   
      }
    break;

    case (0x40):
     I20CONSET = 1<<2; // AA=1 ACK will be returned 
    break;

    case (0x50):
      if(read_count<DATA_LENGTH)
      {
        if(i2cact==READ_ACCEL)
        {
          accel_data[read_count]=I20DAT;
        }
        else
        {
          gyro_data[read_count]=I20DAT;  
        }
        if(read_count==(DATA_LENGTH-2))
        {
          I20CONCLR= 1<<2; // AA=0 
        }
        read_count++;
      }
    break;
    
    case (0x58):
      if(i2cact==READ_ACCEL)
      {
        accel_data[read_count]=I20DAT;
      }
      else
      {
        gyro_data[read_count]=I20DAT;  
      }
           
     I20CONSET = 0x10;  //stop will transmitted
     i2c_bus_busy=0; // free i2c bus
            
    break; 
    
    default:
    {
      I20CONSET = 0x10;  //stop will transmitted
      i2c_bus_busy=0; // free i2c bus
//      accel_data[1]=0x23;
    }
    break;
  }

  I20CONCLR = 0X08;
  VICVectAddr=0x00;
}